# -*- coding: utf-8 -*-
"""
Created on Fri Nov 13 19:39:09 2020

@author: chris
"""

import numpy as np
import cv2
from ctypes import *
import threading
import DobotDllType as dType
from scipy import linalg


def cv_show(image):
    """方便的查看图片"""
    cv2.namedWindow('image', cv2.WINDOW_NORMAL)
    cv2.imshow('image', image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()


def get_image():
    """获取相机的图像，返回一个三维数组"""
    dll = cdll.LoadLibrary("JHCap2.dll")
    dll.CameraInit(0)  # 初始化相机
    dll.CameraSetResolution(0, 0, 0, 0)  # 设置相机分辨率
    dll.CameraSetContrast.argtypes = [c_int, c_double]  # 设置相机对比度
    dll.CameraSetContrast(0, 1.15)  # 设置相机对比度
    buflen = c_int()
    width = c_int()
    height = c_int()
    dll.CameraGetImageSize(0, byref(width), byref(height))  # 获取图像大小
    dll.CameraGetImageBufferSize(0, byref(buflen), 0x4)  # 获取当前图像需要分配的内存大小
    inbuf = create_string_buffer(buflen.value)
    # cv2.namedWindow("s")
    # while 1: 
    dll.CameraQueryImage(0, inbuf, byref(buflen), 0x104)
    arr = np.frombuffer(inbuf, np.uint8)
    image = np.reshape(arr, (height.value, width.value, 3))
    return image


def process(image):
    """图像图片的预处理"""
    ref = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)  # 灰度图
    ref = cv2.threshold(ref, 127, 255, cv2.THRESH_BINARY_INV)[1]  # 阈值处理得到二值化图像
    # ref = cv2.adaptiveThreshold(ref, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 5,3)     # 自适应阈值效果不好
    k = np.ones((20, 20), np.uint8)
    ref = cv2.morphologyEx(ref, cv2.MORPH_CLOSE, k)  # 图像闭运算
    img, contours, hierarchy = cv2.findContours(ref,
                                                cv2.RETR_EXTERNAL,
                                                cv2.CHAIN_APPROX_SIMPLE)  # 计算轮廓
    # contours = cv2.drawContours(image, contours, -1, (0,0,255), 3)     # 画出轮廓
    contours_num = len(contours)
    contours_image = []

    return ref, contours, contours_image, contours_num


def callback(x):
    pass


def circle_center(image):
    """霍夫变换圆检测,返回圆心坐标数组 """

    # 定义滚动条
    cv2.namedWindow('image', cv2.WINDOW_NORMAL)
    cv2.createTrackbar('minD', 'image', 5, 300, callback)
    cv2.createTrackbar('param1', 'image', 5, 300, callback)
    cv2.createTrackbar('param2', 'image', 5, 300, callback)
    cv2.createTrackbar('minR', 'image', 5, 100, callback)
    cv2.createTrackbar('maxR', 'image', 5, 200, callback)
    cv2.setTrackbarPos('minD', 'image', 10)
    cv2.setTrackbarPos('param1', 'image', 100)
    cv2.setTrackbarPos('param2', 'image', 50)
    cv2.setTrackbarPos('minR', 'image', 30)
    cv2.setTrackbarPos('maxR', 'image', 100)
    # 霍夫变换圆检测
    image1 = image
    while True:
        # 载入并显示图片
        image = image1.copy()
        # image = get_image()
        #  image = image1
        # 灰度化
        img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        img = cv2.GaussianBlur(img, (5, 5), 0, 0)
        minD = int(cv2.getTrackbarPos('minD', 'image'))
        param1 = int(cv2.getTrackbarPos('param1', 'image'))
        param2 = int(cv2.getTrackbarPos('param2', 'image'))
        minR = int(cv2.getTrackbarPos('minR', 'image'))
        maxR = int(cv2.getTrackbarPos('maxR', 'image'))
        circles = cv2.HoughCircles(img,
                                   cv2.HOUGH_GRADIENT,
                                   1,
                                   minD,  # 园心间最小间距
                                   param1=param1,
                                   param2=param2,
                                   minRadius=minR,
                                   maxRadius=maxR)
        if circles is not None:
            # print(len(circles[0]))
            # 根据检测到圆的信息，画出每一个圆
            for circle in circles[0]:
                # 坐标行列
                x = int(circle[0])
                y = int(circle[1])
                # 半径
                r = int(circle[2])
                # 在原图用指定颜色标记出圆的位置
                circle_img = cv2.circle(image, (x, y), r, (0, 0, 255), -1)
        cv2.imshow('image', circle_img)
        k = cv2.waitKey(1) & 0xFF
        if k == 27:
            break
    cv2.destroyWindow('image')
    local = circles[:, :, 0:2]  # 返回一个(1,7,2)矩阵
    local = np.reshape(local, (-1, 2))  # 返回一个(7,2)矩阵
    cv_show(image)
    return local


def trans_local(image_local):
    """将相机坐标转换为机械臂坐标
        返回一个一个机械臂坐标的列表"""
    image_center = np.mat([1158.3, 764.57])
    dobot_center = np.mat([173.51, 0.63])   #z=120.5
    dobot_xs = []
    dobot_ys = []
    for image_local_x, image_local_y in image_local:
        dobot_x = ((-image_local_y + image_center[0, 1]) / 16.75) + dobot_center[0, 0]
        dobot_y = ((-image_local_x + image_center[0, 0]) / 16.75) + dobot_center[0, 1]
        dobot_xs.append(dobot_x)
        dobot_ys.append(dobot_y)
    dobot_xs = np.mat(dobot_xs)
    dobot_ys = np.mat(dobot_ys)
    dobot_local = np.append(dobot_xs,dobot_ys, axis=0).T
    return dobot_local.A


def Control_dobot(pearl_local):
    """控制机械臂的运动"""
    CON_STR = {
        dType.DobotConnect.DobotConnect_NoError: "DobotConnect_NoError",
        dType.DobotConnect.DobotConnect_NotFound: "DobotConnect_NotFound",
        dType.DobotConnect.DobotConnect_Occupied: "DobotConnect_Occupied"}

    # 将dll读取到内存中并获取对应的CDLL实例
    api = dType.load()
    # 建立与dobot的连接
    state = dType.ConnectDobot(api, "", 115200)[0]
    print("Connect status:", CON_STR[state])

    # pearl_local = np.array([[250.0, 46.3],
    #                         [275.0, 48.3],
    #                         [290.0, 50.3],
    #                         [250.0, 30.0]])
    #
    dobot_center = np.array([112, 40])
    box_local = np.array([-7, -163])

    if state == dType.DobotConnect.DobotConnect_NoError:
        # 清空队列
        dType.SetQueuedCmdClear(api)

        # 设置运动参数
        dType.SetHOMEParams(api, 200, 200, 200, 200, isQueued=1)
        dType.SetPTPJumpParams(api, 30, 100, isQueued=1)

        dType.SetPTPCommonParams(api, 100, 30, isQueued=1)
        # dType.SetEndEffectorParams(api, 0, 0, 60, isQueued=1)    #设置气泵的状态

        # 回零
        # dType.SetHOMECmd(api, temp=0, isQueued=1)

        # # 设置ptpcmd内容并将命令发送给dobot
        Index_0 = dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode,
                                  dobot_center[0], dobot_center[1],
                                  20, 40, isQueued=1)[0]

        dType.dSleep(1000)
        for dobot_x, dobot_y in pearl_local:
            Index_1 = dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode,
                                      dobot_x, dobot_y, 4, 0, isQueued=1)[0]
            Index_11 = dType.SetWAITCmd(api, 1000, isQueued=1)[0]
            Index_2 = dType.SetEndEffectorSuctionCup(api, 1, 1, isQueued=1)[0]

            Index_3 = dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode,
                                      box_local[0], box_local[1], -10, 0, isQueued=1)[0]
            dType.dSleep(1000)
            Index_4 = dType.SetEndEffectorSuctionCup(api, 0, 1, isQueued=1)[0]
            print(dobot_x, dobot_y)

        Index_5 = dType.SetPTPCmd(api, dType.PTPMode.PTPJUMPXYZMode,
                                  dobot_center[0], dobot_center[1],
                                  20, 40, isQueued=1)[0]

        # 开始执行指令队列
        print('开始')
        dType.SetQueuedCmdStartExec(api)

        # 如果还未完成指令队列则等待
        while Index_5 > dType.GetQueuedCmdCurrentIndex(api)[0]:
            dType.dSleep(100)

        print(0)
        # 停止执行指令
        dType.SetQueuedCmdStopExec(api)
    # 断开连接
    dType.DisconnectDobot(api)


if __name__ == '__main__':
    img = get_image()
    cv_show(img)
    image_pearl_local = circle_center(img)
    dobot_local = trans_local(image_pearl_local)
    # box_local = np.array([23, -150])    # 盒子的位置坐标
    # dobot_center = np.array([100, -3])  # dobot默认位置
    # Control_dobot(dobot_local)
    # pearl_local = np.array([[234.8, -2.13],
    #                         [283.4, -7.4],
    #                         [261.4, 16.14]])
    #
    # box_local = np.array([30, -240]) #z=150
    # dobot_center = np.array([240, -10])
    # Control_dobot(pearl_local, box_local, dobot_center)
